#include <iostream>
#include <algorithm>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

typedef struct Ellipse {
    int accumulator;
    double xc, yc;
    double a, b;
    double angle;
} Ellipse;

static std::vector<double> _create_bins(const std::vector<double>& acc, double binSize) {
    std::vector<double>::const_iterator maxPos = std::max_element(acc.begin(), acc.end());
    double maxVal = *maxPos;
    std::vector<double> v;
    for (double d = 0; d < maxVal + binSize; d += binSize) {
        v.push_back(d);
    }
    return v;
}

static std::vector<int> _histogram(const std::vector<double>& acc, const std::vector<double>& bins) {
    std::vector<int> hist(bins.size()-1);
    for (int i = 0; i < hist.size(); i++) {
        hist[i] = 0;
    }
    for (int i = 0; i < acc.size(); i++) {
        double a = acc[i];
        for (int j = 0; j < bins.size()-1; j++) {
            double left = bins[j];
            double right = bins[j+1];
            if (a >= left && a < right) {
                hist[j]++;
                break;
            }
        }
        if (a == bins[bins.size()-1]) {
            hist[bins.size()-2]++;
        }
    }
    return hist;
}

static std::vector<Ellipse> _hough_ellipse(const cv::Mat& edges, int threshold, double accuracy, int minAxis, int maxAxis=-1, int maxFirst=-1) {
    std::vector<Ellipse> results;
    
    int numRows = edges.rows;
    int numCols = edges.cols;
    std::vector<int> nonzeroXIndices, nonzeroYIndices;
    for (int i = 0; i < numRows; i++) {
        for (int j = 0; j < numCols; j++) {
            if (edges.ptr<uchar>(i)[j] > 0) {
                nonzeroXIndices.push_back(j);
                nonzeroYIndices.push_back(i);
            }
        }
    }
    int numPixels = nonzeroXIndices.size();
    std::vector<double> accumulator;
    double binSize = accuracy*accuracy;
    double maxAxisSquared = maxAxis*maxAxis;
    if (maxAxis == -1) {
        if (numRows < numCols) {
            maxAxisSquared = cvRound(0.5*numRows);
        } else {
            maxAxisSquared = cvRound(0.5*numCols);
        }
        maxAxisSquared *= maxAxisSquared;
    }
    
    int p1, p2, p, x1, y1, x2, y2, x, y;
    double x0, y0, a, b, d, k, dx, dy, fSquared, cosTauSquared, bSquared, alpha;
    for (p1 = 0; p1 < numPixels; p1++) {
        for (p2 = 0; p2 < p1; p2++) {
            x1 = nonzeroXIndices[p1];
            y1 = nonzeroYIndices[p1];
            x2 = nonzeroXIndices[p2];
            y2 = nonzeroYIndices[p2];
            
            dx = x1-x2;
            dy = y1-y2;
            a = 0.5*std::sqrt(dx*dx+dy*dy);
            bool bMaxFirst = (maxFirst == -1);
            if (maxFirst != -1) {
                bMaxFirst = (a <= maxFirst);
            }
            if (a > minAxis && bMaxFirst) {
                x0 = 0.5*(x1+x2);
                y0 = 0.5*(y1+y2);
                for (p = 0; p < numPixels; p++) {
                    x = nonzeroXIndices[p];
                    y = nonzeroYIndices[p];
                    dx = x-x0;
                    dy = y-y0;
                    d = std::sqrt(dx*dx+dy*dy);
                    if (d > minAxis) {
                        dx = x-x2;
                        dy = y-y2;
                        fSquared = dx*dx+dy*dy;
                        cosTauSquared = (a*a+d*d-fSquared) / (2*a*d);
                        cosTauSquared *= cosTauSquared;
                        k = a*a-d*d*cosTauSquared;
                        if (k > 0 && cosTauSquared < 1) {
                            bSquared = a*a*d*d*(1-cosTauSquared) / k;
                            if (bSquared <= maxAxisSquared) {
                                accumulator.push_back(bSquared);
                            }
                        }
                    }
                }
                
                if (accumulator.size() > 0) {
                    std::vector<double> bins;
                    bins = _create_bins(accumulator, binSize);
                    std::vector<int> hist;
                    hist = _histogram(accumulator, bins);
                    std::vector<int>::iterator histMaxIter = std::max_element(hist.begin(), hist.end());
                    int histMax = *histMaxIter;
                    if (histMax > threshold) {
                        alpha = std::atan2(y2-y1, x2-x1);
                        int maxIndex = histMaxIter - hist.begin();
                        b = std::sqrt(bins[maxIndex]);
                        if (alpha != 0) {
                            alpha = CV_PI - alpha;
                        }
                        if (a > 0 && b > 0) {
                            Ellipse ellipse;
                            ellipse.accumulator = histMax;
                            ellipse.xc = x0;
                            ellipse.yc = y0;
                            ellipse.a = a;
                            ellipse.b = b;
                            ellipse.angle = alpha * 180 / CV_PI;
                            results.push_back(ellipse);
                        }
                    }
                    accumulator.clear();
                }
            }
        }
    }
    
    return results;
}

static bool _hough_ellipse2(int numRows, int numCols, const std::vector<cv::Point>& contour, double threshold, cv::Point& center, float& a, float& b, float& theta) {
    cv::Mat maxDists(numRows, numCols, CV_32FC1, cv::Scalar::all(0));
    std::vector<float> dists(contour.size());
    for (int i = 0; i < numRows; i++) {
        for (int j = 0; j < numCols; j++) {
            for (int k = 0; k < contour.size(); k++) {
                float dx = j-contour[k].x;
                float dy = i-contour[k].y;
                float dist = std::sqrt(dx*dx+dy*dy);
                dists[k] = dist;
            }
            std::vector<float>::iterator iter = std::max_element(dists.begin(), dists.end());
            float maxDist = *iter;
            maxDists.ptr<float>(i)[j] = maxDist;
        }
    }
    
    double minVal;
    cv::Point minLoc;
    cv::minMaxLoc(maxDists, &minVal, NULL, &minLoc);
    center = minLoc;
    a = (float)minVal;
    
    cv::Mat houghSpace(std::floor(a+1), 180, CV_32SC1, cv::Scalar::all(0));
    for (int i = 0; i < contour.size(); i++) {
        for (int j = 0; j < 180; j++) {
            double angle = j*CV_PI/180;
            double p = center.x;
            double q = center.y;
            double x = contour[i].x;
            double y = contour[i].y;
            double cosTheta = std::cos(angle);
            double sinTheta = std::sin(angle);
            double part1 = std::pow((x-p)*cosTheta+(y-q)*sinTheta, 2) / (a*a);
            double part2 = std::pow(-(x-p)*sinTheta+(y-q)*cosTheta, 2);
            int B = std::floor(std::sqrt(part2 / (1-part1)) + 1);
            if (B > 0 && B <= a) {
                houghSpace.ptr<int>(B)[j] += 1;
            }
        }
    }
    
    double maxVal;
    cv::Point maxLoc;
    cv::minMaxLoc(houghSpace, NULL, &maxVal, NULL, &maxLoc);
    if (maxVal >= threshold) {
        b = maxLoc.y;
        theta = maxLoc.x;
        return (a > 1 && b > 1);
    }
    return false;
}

int main(int argc, char **argv) {
    /*cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/pic3.png", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    cv::Mat gray;
    cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
    cv::Mat gb;
    cv::GaussianBlur(gray, gb, cv::Size(7, 7), 2, 2);
    int kernelSize = 3;
    double cannyThresh = 100;
    cv::Mat edges, dx, dy;
    cv::Sobel(gb, dx, CV_16SC1, 1, 0, kernelSize, 1, 0, cv::BORDER_REPLICATE);
    cv::Sobel(gb, dy, CV_16SC1, 0, 1, kernelSize, 1, 0, cv::BORDER_REPLICATE);
    cv::Canny(dx, dy, edges, cannyThresh/2, cannyThresh, false);
    
    int threshold = 135;
    double accuracy = 15;
    int minAxis = 30;
    int maxFirst = 48;
    int maxAxis = 58;
    std::vector<Ellipse> ellipses;
    ellipses = _hough_ellipse(edges, threshold, accuracy, minAxis, maxAxis, maxFirst);
    std::cout << "Found " << ellipses.size() << " ellipses." << std::endl;
    
    cv::Mat dst = src.clone();
    for (int i = 0; i < ellipses.size(); i++) {
        Ellipse ellipse = ellipses[i];
        cv::ellipse(dst, cv::Point(cvRound(ellipse.xc), cvRound(ellipse.yc)), cv::Size(cvRound(ellipse.a), cvRound(ellipse.b)), ellipse.angle, 0, 360, cv::Scalar(255, 0, 255), 2, cv::LINE_AA);
        std::cout << "Ellipse " << i << ": xc = " << cvRound(ellipse.xc) << ", yc = " << cvRound(ellipse.yc) << ", a = " << cvRound(ellipse.a) << ", b = " << cvRound(ellipse.b) << ", angle = " << ellipse.angle << ", accumulator = " << ellipse.accumulator << std::endl;
    }
    cv::imshow("src", src);
    cv::imshow("edges", edges);
    cv::imshow("dst", dst);
    cv::waitKey(0);*/
    
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/detect_blob.png", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    cv::Mat gray;
    cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
    cv::Mat gb;
    cv::GaussianBlur(gray, gb, cv::Size(3, 3), 2, 2);
    int kernelSize = 3;
    double cannyThresh = 35;
    cv::Mat edges, dx, dy;
    cv::Sobel(gb, dx, CV_16SC1, 1, 0, kernelSize, 1, 0, cv::BORDER_REPLICATE);
    cv::Sobel(gb, dy, CV_16SC1, 0, 1, kernelSize, 1, 0, cv::BORDER_REPLICATE);
    cv::Canny(dx, dy, edges, cannyThresh/2, cannyThresh, false);
    
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(edges, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
    cv::Mat dst = src.clone();
    for (int i = 0; i < contours.size(); i++) {
        float a, b, theta;
        cv::Point center;
        double threshold = contours[i].size() * 0.33;
        if (_hough_ellipse2(dst.rows, dst.cols, contours[i], threshold, center, a, b, theta)) {
            cv::ellipse(dst, cv::Point(center.x, center.y), cv::Size(cvRound(a), cvRound(b)), theta, 0, 360, cv::Scalar(255, 0, 255), 2, cv::LINE_AA);
            std::cout << "center = (" << center.x << ", " << center.y << "), a = " << a << ", b = " << b << ", theta = " << theta << std::endl;
        }
    }
    cv::imshow("src", src);
    cv::imshow("edges", edges);
    cv::imshow("dst", dst);
    cv::waitKey(0);
    
    return 0;
}
